/********************************************************************************
 * Copyright(c) 2020-2025 VINCENT_WY All rights reserved.
 * - Filename
 * - Author  Vincent
 * - Version V1.0.0
 * - Date    2023/09/21
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "NoBufferedCanMsgReplayImpl.h"
#include <fstream>
#include "logutil.h"
#include "nlohmann/json.hpp"

using namespace std;
using vjson = nlohmann::json;

NoBufferedCanMsgReplayImpl::NoBufferedCanMsgReplayImpl(const bool &enbale, const std::string &path)
    : Tag("[NoBufferedCanMsgReplayImpl]: "), _usrCanReplay(enbale),
      _root(path)
{
    if (!_usrCanReplay)
        return;
    LOGD("%s read can msg file ...", Tag.c_str());
    ReadFile();
    LOGD("%s Create ...", Tag.c_str());
}

NoBufferedCanMsgReplayImpl::~NoBufferedCanMsgReplayImpl()
{
    LOGD("%s Destroyed ...", Tag.c_str());
}

void NoBufferedCanMsgReplayImpl::start(const long long int &time)
{
    if (!_usrCanReplay)
        return;
        
    if (_canFileList.empty())
    {
        LOGE("%s No Can msg file in list !!!", Tag.c_str());
        _usrCanReplay = false;
        return;
    }
    #ifndef DISABLE_CYBER
    // cyber init
    apollo::cyber::Init("CanTakler");
    _canMsgTalkerNode = apollo::cyber::CreateNode("ReplayCanTalker");
    if (!_canMsgTalkerNode)
    {
        LOGD("%s create node err!!!", Tag.c_str());
        return;
    }
    _canMsgTalker = _canMsgTalkerNode->CreateWriter<autoplt::can>("/can_chatter");
    if (!_canMsgTalker)
    {
        LOGD("%s create cyber writer err!!!", Tag.c_str());
        return;
    }
    LOGD("%s doInitCyberRT OK!", Tag.c_str());
    //
    #endif
    _startTime = time;
    _readLoop = std::make_unique<std::thread>(&NoBufferedCanMsgReplayImpl::ReadLoop, this);
    _invokeLoop =
        std::make_unique<std::thread>(&NoBufferedCanMsgReplayImpl::InvokeLoop, this);
    _status = Status::START;
}
bool NoBufferedCanMsgReplayImpl::isComplete()
{
    if (!_usrCanReplay)
        return true;
    return _isComplete;
}

long long int NoBufferedCanMsgReplayImpl::getFirstFrameTm()
{
    if (_canFileList.empty())
        return -1;
    return _canFileList[0].first;
}

void NoBufferedCanMsgReplayImpl::doRepeat()
{
    if (!_usrCanReplay)
        return;
    _status = Status::REPEAT;
}

void NoBufferedCanMsgReplayImpl::stop()
{
    if (!_usrCanReplay)
        return;
    LOGD("%s Stop 1...", Tag.c_str());
    _status = Status::IDLE;
    _isExit = true;
    _frameCv.notify_all();
    if ((_readLoop != nullptr) && _readLoop->joinable())
    {
        _readLoop->join();
    }
    if ((_invokeLoop != nullptr) && _invokeLoop->joinable())
    {
        _invokeLoop->join();
    }
    LOGD("%s Stop 2...", Tag.c_str());
}

namespace Bavm
{
#undef NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE
#define NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(Type, ...)                                                                                                                      \
    inline void to_json(nlohmann::ordered_json &nlohmann_json_j, const Type &nlohmann_json_t) { NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_TO, __VA_ARGS__)) } \
    inline void from_json(const nlohmann::ordered_json& nlohmann_json_j, Type& nlohmann_json_t) { const Type nlohmann_json_default_obj{}; NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(NLOHMANN_JSON_FROM_WITH_DEFAULT, __VA_ARGS__)) }
    NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(CanMsg, time, version_request, calib_request, speed_valid, speed_km_h, wheel_pulse_front_left_valid, wheel_pulse_front_left, wheel_pulse_front_right_valid, wheel_pulse_front_right, wheel_pulse_rear_left_valid, wheel_pulse_rear_left, wheel_pulse_rear_right_valid, wheel_pulse_rear_right, steer_angle_valid, steer_angle_deg, yaw_rate_valid, yaw_rate_deg_per_sec, left_turn_switch_valid, left_turn_switch_on, right_turn_switch_valid, right_turn_switch_on, brake_valid, brake_on, gear_valid, gear_state, parking_status, dr_result_t, dr_result_valid, dr_result_pose_x_m, dr_result_pose_y_m, dr_result_pose_theta_rad,
        uss_data_valid,m_front_radar1_status,m_front_radar2_status,m_front_radar3_status,m_front_radar4_status,m_rear_radar1_status,m_rear_radar2_status,m_rear_radar3_status,m_rear_radar4_status,m_left_radar1_status,m_left_radar2_status,m_left_radar3_status,m_left_radar4_status,m_right_radar1_status,m_right_radar2_status,m_right_radar3_status,m_right_radar4_status)
}

void NoBufferedCanMsgReplayImpl::ReadLoop()
{
    int index = 0;
    long long int mLastTimeStamp{};

    while (!_isExit)
    {
        switch (_status)
        {
        case Status::START:
        {
            index = 0;
            _status = Status::CONTINUE;
            mLastTimeStamp = _startTime;
            _isComplete = false;
        }
        break;

        case Status::REPEAT:
        {
            _status = Status::START;
        }
        break;

        case Status::CONTINUE:
        {
            unsigned long long frameInterval = _canFileList[index].first - mLastTimeStamp;
            unsigned long long t1{}, t2{}, readInterval{};

            t1 = TimeUtil::now_us();
            vjson mJsonString;
            std::ifstream mJsonFileIn(_canFileList[index].second);
            if (!mJsonFileIn.is_open())
            {
                LOGE("%s failed to open [%d]", Tag.c_str(),
                     _canFileList[index].second.c_str());
                mJsonFileIn.close();
                index++;
                continue;
            }
            try
            {
                mJsonFileIn >> mJsonString;
            }
            catch (vjson::exception &e)
            {
                LOGE("%s [%d] parse error", Tag.c_str(),
                     _canFileList[index].second.c_str());
                mJsonFileIn.close();
                index++;
                continue;
            }
            mJsonFileIn.close();
            Bavm::CanMsg mCanMsg = mJsonString.get<Bavm::CanMsg>();

            // 等待两帧之间的时间间隔，时间是us
            t2 = TimeUtil::now_us();
            readInterval = t2 - t1;
            LOGD("Replay get can_msg frame timestamp %ld, frameInterval %llu, readInterval %llu\n",
                _canFileList[index].first, frameInterval, readInterval);
            if (frameInterval > readInterval) {
                std::this_thread::sleep_for(std::chrono::microseconds(10));
            }

            {
                std::lock_guard<std::mutex> lock(_frameMtx);
                _frameDqu.push_back(std::move(mCanMsg));
            }

            _frameCv.notify_one();

            mLastTimeStamp = _canFileList[index].first;
            index++;
            if (index >= (int)_canFileList.size())
            {
                _status = Status::IDLE;
                _isComplete = true;
                index = 0;
                LOGD("%s can complete.\n", Tag.c_str());
            }
        }
        break;

        case Status::IDLE:
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(3));
        }
        break;

        default:
            break;
        }
    }
}

void NoBufferedCanMsgReplayImpl::InvokeLoop()
{
    while (!_isExit)
    {
        std::unique_lock<std::mutex> lock(_frameMtx);
        _frameCv.wait(lock, [this]
                      { return _frameDqu.size() > 0 || _isExit; });
        if (_isExit)
            break;
        auto frame = _frameDqu.front();
        _frameDqu.pop_front();
        lock.unlock();
        // bavm can msg to cyber can msg
        autoplt::can mCyberCanMsg;
        mCyberCanMsg.set_brake_on(frame.brake_on);
        mCyberCanMsg.set_brake_valid(frame.brake_valid);
        mCyberCanMsg.set_calib_request(frame.calib_request);
        mCyberCanMsg.set_gear_state(frame.gear_state);
        mCyberCanMsg.set_gear_valid(frame.gear_valid);
        mCyberCanMsg.set_left_turn_switch_on(frame.left_turn_switch_on);
        mCyberCanMsg.set_left_turn_switch_valid(frame.left_turn_switch_valid);
        mCyberCanMsg.set_right_turn_switch_on(frame.right_turn_switch_on);
        mCyberCanMsg.set_right_turn_switch_valid(frame.right_turn_switch_valid);
        mCyberCanMsg.set_speed_km_h(frame.speed_km_h);
        mCyberCanMsg.set_speed_valid(frame.speed_valid);
        mCyberCanMsg.set_steer_angle_deg(frame.steer_angle_deg);
        mCyberCanMsg.set_steer_angle_valid(frame.steer_angle_valid);
        mCyberCanMsg.set_timestamp(frame.time);
        mCyberCanMsg.set_version_request(frame.version_request);
        mCyberCanMsg.set_yaw_rate_deg_per_sec(frame.yaw_rate_deg_per_sec);
        mCyberCanMsg.set_yaw_rate_valid(frame.yaw_rate_valid);
        mCyberCanMsg.set_parking_status((autoplt::ParkingStatus)frame.parking_status);
        #define ADD_MEMBER(d)  mCyberCanMsg.set_##d(frame.d);
        ADD_MEMBER(uss_data_valid) 
        ADD_MEMBER(m_front_radar1_status) ADD_MEMBER(m_front_radar2_status) 
        ADD_MEMBER(m_front_radar3_status) ADD_MEMBER(m_front_radar4_status)
        ADD_MEMBER(m_rear_radar1_status) ADD_MEMBER(m_rear_radar2_status)
        ADD_MEMBER(m_rear_radar3_status) ADD_MEMBER(m_rear_radar4_status)
        ADD_MEMBER(m_left_radar1_status) ADD_MEMBER(m_left_radar2_status)
        ADD_MEMBER(m_left_radar3_status) ADD_MEMBER(m_left_radar4_status)
        ADD_MEMBER(m_right_radar1_status) ADD_MEMBER(m_right_radar2_status)
        ADD_MEMBER(m_right_radar3_status) ADD_MEMBER(m_right_radar4_status)

        LOGD("Replay send can_msg frame timestamp %ld", (uint64_t)(frame.time * 1000000));
        #ifndef DISABLE_CYBER
        _canMsgTalker->Write(mCyberCanMsg);
        #endif
    }
}

void NoBufferedCanMsgReplayImpl::ReadFile()
{
    std::string mRoot = _root;
    if (mRoot.back() != '/')
    {
        mRoot.append("/");
    }
    DIR *pDir;
    struct dirent *ptr;
    if (!(pDir = opendir(mRoot.c_str())))
    {
        LOGE("Folder doesn't Exist!");
        return;
    }

    std::map<int, std::string> fileMap;
    while ((ptr = readdir(pDir)) != 0)
    {
        if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
        {
            std::string mFileName = ptr->d_name;
            long long int mTimestamp{0};
            sscanf(mFileName.c_str(), "%lld.json", &mTimestamp);
            _canFileList.emplace_back(std::make_pair(mTimestamp, mRoot + mFileName));
        }
    }
    closedir(pDir);
    sort(_canFileList.begin(), _canFileList.end());
}
